#include "hdmap_layer_api.h"
//#include "AutoSDK.hpp"
//#include "hdmap/IHDMap.hpp"
//#include "hdmap/Road.hpp"
#include "IMPAuto.hpp"
#include "dataquery/NIDataQueryHeaderAll.h"
#include "base/defines.h"
#include "base/coordRef.h"
#include "base/common.h"
#include "base/log.h"
//#include <eigen3/Eigen/Core>
#include <iostream>
#include <vector>

using namespace MAPSDK;

namespace MAPSDK
{
    class HdMapLayerImpl
    {
    public:
        HdMapLayerImpl()
        {
            autosdk_ = CreateIMPAutoSDK();
        }

        ~HdMapLayerImpl()
        {
            autosdk_->Release();
            autosdk_ = nullptr;
        }

        bool MapInit(const std::string &map_path)
        {
            if (autosdk_->Config(map_path) != Navinfo::IMPAuto::Common::RETURN_CODE::SUCCESS)
            {
                std::cout << "autosdk configuration failed!" << std::endl;
                return false;
            }
            autosdk_->Start();
            /*if (!autosdk_->Start() != Navinfo::IMPAuto::Common::RETURN_CODE::SUCCESS)
            {
                std::cout << "autosdk start failed!   xueyufei" << std::endl;
                return false;
            }
            map_handler_ = autosdk_->GetHDMap();*/
            map_handler_ = GetDataQuery();
            if (!map_handler_)
            {
                std::cout << "fail to get hdmap handler!" << std::endl;
                return false;
            }

            if(map_handler_->CreateAssist(spIDataQueryAssist_) != Navinfo::IMPAuto::Common::SUCCESS)
            {
                std::cout << "sdk create assist failed" << std::endl;
                return false;
            }
            return true;
        }

        std::vector<hdmap_sign> SearchSignsByRadius(const GeoPoint &gps_point, double radius)
        {
            Navinfo::Nis::CCoord2D clPos;
            clPos.m_iX = Navinfo::IMPAuto::Util::CNICoordConv::Degree2NICoord(gps_point.longitude);
            clPos.m_iY = Navinfo::IMPAuto::Util::CNICoordConv::Degree2NICoord(gps_point.latitude);

            std::vector<hdmap_sign> hdmap_signs;
            std::vector<Navinfo::Nis::CObjectId> vcSignLists;
            auto status = spIDataQueryAssist_->SearchObjectByRadiusRange(clPos, radius, Navinfo::Nis::OBJECT_TYPE::OBJECT_SIGN_ALL, vcSignLists);
            if(status == Navinfo::IMPAuto::Common::SUCCESS && !vcSignLists.empty())
            {
                for(Navinfo::Nis::CObjectId sign_id : vcSignLists)
                {
                    std::shared_ptr<Navinfo::IMPAuto::DataQuery::IObjectSignIterator> spISignAttributeIterator;
                    if (map_handler_->CreateObjectSignIterator(spISignAttributeIterator, sign_id) == Navinfo::IMPAuto::Common::RETURN_CODE::SUCCESS)
                    {
                        vector<Navinfo::Nis::CCoord3D> geometry_points = spISignAttributeIterator->GetOutLine();

                        hdmap_sign sign;
                        for (const Navinfo::Nis::CCoord3D &v : geometry_points)
                        {
                            double longitude = Navinfo::IMPAuto::Util::CNICoordConv::NICoord2Degree(v.m_iX);
                            double latitude = Navinfo::IMPAuto::Util::CNICoordConv::NICoord2Degree(v.m_iY);
                            double altitude = v.m_iZ;
                            
                            double x, y, z;
                            CoordRef::Instance().WGS2GKP(longitude, latitude, altitude, x, y, z);
                            sign.geometry.emplace_back(x, y, z);
                        }
                        hdmap_signs.push_back(sign);
                    }
                }
            }
            return hdmap_signs;
        }

        std::vector<hdmap_lane> SearchLanesByRadius(const GeoPoint &gps_point, double radius)
        {
            Navinfo::Nis::CCoord2D clPos;
            clPos.m_iX = Navinfo::IMPAuto::Util::CNICoordConv::Degree2NICoord(gps_point.longitude);
            clPos.m_iY = Navinfo::IMPAuto::Util::CNICoordConv::Degree2NICoord(gps_point.latitude);

            std::vector<hdmap_lane> hdmap_lanes;
            std::vector<Navinfo::Nis::CLaneBoundaryId> vcLaneBoundaryIdList;
            auto status = spIDataQueryAssist_->SearchLaneBoundaryByRadiusRange(clPos, radius, vcLaneBoundaryIdList);
            if(status == Navinfo::IMPAuto::Common::SUCCESS && !vcLaneBoundaryIdList.empty())
            {
                for(Navinfo::Nis::CLaneBoundaryId lane_id : vcLaneBoundaryIdList)
                {
                    std::shared_ptr<Navinfo::IMPAuto::DataQuery::ILaneBoundaryAttributeIterator> spILaneBoundaryAttributeIterator;
                    if(map_handler_->CreateLaneBoundaryAttributeIterator(spILaneBoundaryAttributeIterator, lane_id) == Navinfo::IMPAuto::Common::RETURN_CODE::SUCCESS)
                    {
                        Navinfo::Nis::CLaneBoundaryParallelElementGroup LBPEgroup = spILaneBoundaryAttributeIterator->GetLaneBoundary();
                        std::vector<Navinfo::Nis::CLaneBoundaryParallelElement> LBPEs = LBPEgroup.GetLaneBoundaryParallelElement();
                   
                        int mark_type = 0;
                        for(Navinfo::Nis::CLaneBoundaryParallelElement LBPE : LBPEs)
                        {
                            std::vector<Navinfo::Nis::CLaneBoundaryElement> laneboundelements = LBPE.GetLaneBoundaryElementList();
                            for(Navinfo::Nis::CLaneBoundaryElement laneboundelement : laneboundelements)
                            {
                                if(laneboundelement.GetLaneBoundaryType() == Navinfo::Nis::LANE_BOUNDARY_TYPE::LANE_BOUNDARY_TYPE_LONG_DASHED_LINE)
                                {
                                    mark_type = 2;
                                    break;
                                }
                                else if(laneboundelement.GetLaneBoundaryType() == Navinfo::Nis::LANE_BOUNDARY_TYPE::LANE_BOUNDARY_TYPE_SINGLE_SOLID_LINE)
                                {
                                    mark_type = 1;
                                    break;
                                }
                                else if(laneboundelement.GetLaneBoundaryType() == Navinfo::Nis::LANE_BOUNDARY_TYPE::LANE_BOUNDARY_TYPE_SHORT_DASHED_LINE)
                                {
                                    mark_type = 4;
                                    break;
                                }
                            }
                            if (mark_type != 0)
                            {
                                break;
                            }
                        }

                        if (mark_type == 0)
                        {
                            continue;
                        }

                        std::vector<Eigen::Vector3d> geometry;
                        const std::vector<Navinfo::Nis::CCoord3D> &geometry_points = LBPEgroup.GetGeometry().GetCoordList().GetCoord();
                        for (const Navinfo::Nis::CCoord3D &v : geometry_points)
                        {
                            double longitude = Navinfo::IMPAuto::Util::CNICoordConv::NICoord2Degree(v.m_iX);
                            double latitude = Navinfo::IMPAuto::Util::CNICoordConv::NICoord2Degree(v.m_iY);
                            double altitude = v.m_iZ;
                            
                            double x, y, z;
                            CoordRef::Instance().WGS2GKP(longitude, latitude, altitude, x, y, z);
                            geometry.emplace_back(x, y, z);
                        }

                        hdmap_lane lane;
                        lane.id = lane_id.GetLaneBoundaryNo();
                        lane.geometry = geometry;
                        lane.mark_type = mark_type;
                        hdmap_lanes.push_back(std::move(lane));
                    }
                }
            }
            return hdmap_lanes;
        }

    private:
        //Navinfo::Engine::AutoSDK *autosdk_;
        //Navinfo::Engine::IHDMap *map_handler_;
        Navinfo::IMPAuto::SDKControl *autosdk_;
        Navinfo::IMPAuto::DataQuery::IDataQueryManager *map_handler_;
        std::shared_ptr<Navinfo::IMPAuto::DataQuery::IDataQueryAssist> spIDataQueryAssist_;
    };

} // namespace MAPSDK

HDMapLayer::HDMapLayer()
{
    map_layer_impl_ = new HdMapLayerImpl;
    map_layer_impl_->MapInit("../config/config.ini");
}

HDMapLayer::~HDMapLayer()
{
    delete map_layer_impl_;
    map_layer_impl_ = nullptr;
}

std::vector<hdmap_lane> HDMapLayer::SearchLanesByRadius(const GeoPoint &gps_point, double radius)
{
    return map_layer_impl_->SearchLanesByRadius(gps_point, radius);
}

std::vector<hdmap_sign> HDMapLayer::SearchSignsByRadius(const GeoPoint &gps_point, double radius)
{
    return map_layer_impl_->SearchSignsByRadius(gps_point, radius);
}

hdmap_frame HDMapLayer::SearchByRadius(const GeoPoint &gps_point, double radius)
{
    hdmap_frame hdmap_f;
    hdmap_f.hdmap_lanes = SearchLanesByRadius(gps_point, radius * 100.0);
    hdmap_f.hdmap_signs = SearchSignsByRadius(gps_point, radius * 100.0);
    return hdmap_f;
}